#pragma once

#include <opencv2/opencv.hpp>
#include <cmath>
#include "native_debug.h"


namespace r3d_depth_image {
	// basic types for rendering
#define TAKEN true
#define FREE false

#define PI 3.1415926535897932384626

#define CHECK_ZERO(x) (std::fabs(x)<1e-5)
#define CHECK_NONZERO(x) (!CHECK_ZERO(x))
#define CHECK_ZERO_VEC(t) (CHECK_ZERO(t.x*t.x+t.y*t.y+t.z*t.z))
#define CHECK_NONZERO_VEC(t) (!CHECK_ZERO_VEC(t))

#define ASSERT_ZERO(x) {if(CHECK_NONZERO(x)){\
IMAGE_DEPTH_LOGE("[ASSERTION FAILURE] \
expect zero at %s:%d", __FILE__, __LINE__);FAIL_EXIT(-1);}} 
#define ASSERT_NONZERO(x) {if(CHECK_ZERO(x)){\
IMAGE_DEPTH_LOGE("[ASSERTION FAILURE] \
expect nonzero at %s:%d", __FILE__, __LINE__);FAIL_EXIT(-1);}} 

#define ASSERT_ZERO_VEC(x) {if(CHECK_NONZERO_VEC(x)){\
IMAGE_DEPTH_LOGE("[ASSERTION FAILURE] \
expect zero vector at %s:%d", __FILE__, __LINE__);FAIL_EXIT(-1);}} 
#define ASSERT_NONZERO_VEC(x) {if(CHECK_ZERO_VEC(x)){\
IMAGE_DEPTH_LOGE("[ASSERTION FAILURE] \
expect nonzero vector at %s:%d", __FILE__,__LINE__);FAIL_EXIT(-1);}}



	float normalize(cv::Point3f & v);
	bool check_inversion(
		const cv::Mat & mat1, 
		const cv::Mat & mat2);
	cv::Point3f rotate_vector(
		const cv::Point3f & vec, 
		const cv::Mat & rot);
	cv::Mat get_axis_rot(
		const cv::Point3f & axis, 
		float degree);
	void update_rot_by_orts(
		cv::Mat & rot, 
		const cv::Point3f & ort_x,
		const cv::Point3f & ort_y,
		const cv::Point3f & ort_z);


	class Triangle {
		Triangle() = default;
		Triangle(const Triangle & rhs) = default;
		Triangle(Triangle && rhs) {
			std::swap(verts, rhs.verts);
			std::swap(rgba, rhs.rgba);
		}
		~Triangle() = default;
	public:
		cv::Point3f verts[3];
		uint8_t rgba[4];
	};

	// Camera is based on a XYZ coordinate system as such:
	//					/\ (Z)
	//                 /
	//                /
	//               /_________\ (X)
	//               |
	//               |
	//               |
	//              \| (Y)
	// i.e. \vec{n}_1 = (1,0,0); \vec{n}_2 = (0,1,0);
	//      then \vec{n}_3 = \vec{n}_1 \times \vec{n}_2 = (0,0,1) 
	// that is, X is from left to right;
	//          Y is from top to down;
	//          Z is from outside to inside
	class Camera
	{
	public:
		Camera(
			const cv::Point3f & pos, // position
			const cv::Point3f & ort_x, // orientation of X
			const cv::Point3f & ort_y, // orientation of Y
			const cv::Mat & cmm, // camera matrix
			const cv::Size size // size of captured picture
			);

		// setter
		void SetCameraMatrix(const cv::Mat & cmm);
		void SetPosition(cv::Point3f pos);
		void SetOrientation(cv::Point3f ort_x, cv::Point3f ort_y);
		void SetPictureSize(const cv::Size & size);
		void SetBoundState(bool state);

		// getter
		void GetCameraMatrix(cv::Mat & cmm);
		cv::Point3f GetPosition();
		cv::Point3f GetOrientationX();
		cv::Point3f GetOrientationY();
		cv::Size GetPictureSize();
		void GetLastFrame(cv::Mat & frame);

		// check camera bounded state
		bool IsBoundToTarget();
		cv::Point3f GetTargetPosition();

		// capture: two way to process this, sparse and dense
		// sparse: calculate the cross of ray from target
		// dense: calculate the cross of ray from camera
		// for point cloud, use the first, otherwise the second
		void CapturePoints(
			const cv::Mat & world, 
			cv::Mat & view, 
			cv::Mat & depth);
		void CaptureTriangles(
			const std::vector<Triangle> & tris, 
			cv::Mat & view);
		
		// real world coordinate based ops
		void LookAt(cv::Point3f target);
		void Move(cv::Point3f vec);
		void Rotate(const cv::Mat & rot_);
		
		// camera coordinate based ops
		void Yaw(float A);
		void Pitch(float A);
		void Roll(float A);
		void MoveAhead(float dist);
		void MoveRight(float dist);
		void MoveUp(float dist);

		~Camera();

	private:
		cv::Mat m_view; // current buffered view
		cv::Mat m_view_depth; // log the depth of imaging points
		volatile bool m_view_lock; // make rendering thread safe
		
		cv::Mat m_cmm; // camera matrix
		cv::Point3f m_pos; // current camera postion
		cv::Point3f m_ort_x; // current camera X orientation
		cv::Point3f m_ort_y; // current camera Y orientation
		cv::Point3f m_ort_z; // current camera Z orientation
		cv::Mat m_rot; // camera rotation matrix

		bool m_bound; // if bound to target
		cv::Point3f m_tar; // the position of bound target

		void RotateAxis(cv::Point3f vec, float A);
		void RotateAxis(
			const cv::Point3f & center,
			const cv::Point3f & axis,
			float degree);
	};

}
